import rclpy
from rclpy.node import Node
from unitree_api.msg import Request
import json
import signal
import sys

##实现原地转圈，当然可以自定义修改参数实现其他行为
class Go2Ctrl(Node):
    def __init__(self):
        super().__init__('go2_ctrl_node_py')
        
        # 创建一个AGIROS Publisher
        self.req_puber = self.create_publisher(Request, '/api/sport/request', 10)
        
        # 创建一个定时器，每100毫秒调用一次cruise函数
        self.timer = self.create_timer(0.1, self.cruise)
        
        # 注册信号处理函数
        signal.signal(signal.SIGINT, self.signal_handler)

    def cruise(self):
        req = Request()  # 创建一个运动请求msg
        req.header.identity.api_id = 1008
        # 或者直接使用字符串
        req.parameter = '{"x": 0.0, "y": 0.0, "z": 0.6}'
        self.get_logger().info(f'req.param = {req.parameter}')
        self.req_puber.publish(req)  # 发布数据

    def send_stop_command(self):
        """发送停止命令"""
        req = Request()
        req.header.identity.api_id = 1002  # 停止命令ID
        req.parameter = '{}'  # 空参数
        self.get_logger().info("Sending stop command...")
        self.req_puber.publish(req)
        # 确保消息发送出去
        rclpy.spin_once(self, timeout_sec=0.1)

    def signal_handler(self, sig, frame):
        """处理Ctrl+C信号"""
        self.get_logger().info("检测到用户按下ctrl+c, 发送机器人停止命令")
        self.send_stop_command()
        self.destroy_node()
        rclpy.shutdown()
        sys.exit(0)

def main(args=None):
    # 初始化ROS2客户端
    rclpy.init(args=args)

    # 创建节点对象
    go2_ctrl = Go2Ctrl()

    try:
        # 运行节点
        rclpy.spin(go2_ctrl)
    except KeyboardInterrupt:
        go2_ctrl.get_logger().info("KeyboardInterrupt detected")
    finally:
        # 资源释放
        go2_ctrl.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()